/*
 * Copyright (c) 2011-2025, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/main/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#include <dart8/common/exceptions.hpp>
#include <dart8/comps/joint.hpp>
#include <dart8/multi_body/multi_body.hpp>
#include <dart8/world.hpp>

#include <gtest/gtest.h>

// Test MultiBody creation with explicit name
TEST(MultiBody, CreationWithName)
{
  dart8::World world;
  auto mb = world.addMultiBody("multibody");

  EXPECT_EQ(mb.getName(), "multibody");
  EXPECT_EQ(mb.getLinkCount(), 0u);
  EXPECT_EQ(mb.getJointCount(), 0u);
  EXPECT_EQ(mb.getDOFCount(), 0u);
}

// Test MultiBody creation with auto-generated name
TEST(MultiBody, AutoGeneratedName)
{
  dart8::World world;

  auto mb1 = world.addMultiBody();
  auto mb2 = world.addMultiBody();

  EXPECT_EQ(mb1.getName(), "multibody_001");
  EXPECT_EQ(mb2.getName(), "multibody_002");
}

// Test mixed explicit and auto-generated names (duplicates OK)
TEST(MultiBody, MixedNamesAndDuplicates)
{
  dart8::World world;

  auto mb1 = world.addMultiBody("robot"); // Explicit (counter=1)
  auto mb2 = world.addMultiBody();        // Auto: "multibody_002" (counter=2)
  auto mb3 = world.addMultiBody("robot"); // Duplicate name OK! (counter=3)
  auto mb4 = world.addMultiBody();        // Auto: "multibody_004" (counter=4)

  EXPECT_EQ(mb1.getName(), "robot");
  EXPECT_EQ(mb2.getName(), "multibody_002");
  EXPECT_EQ(mb3.getName(), "robot"); // Duplicate allowed
  EXPECT_EQ(mb4.getName(), "multibody_004");
}

// Test MultiBody creation with different names
TEST(MultiBody, CustomNames)
{
  dart8::World world;

  auto robot = world.addMultiBody("ur5");
  EXPECT_EQ(robot.getName(), "ur5");

  auto character = world.addMultiBody("humanoid");
  EXPECT_EQ(character.getName(), "humanoid");

  auto mechanism = world.addMultiBody("cabinet");
  EXPECT_EQ(mechanism.getName(), "cabinet");
}

// Test MultiBody name get/set
TEST(MultiBody, NameGetSet)
{
  dart8::World world;
  auto mb = world.addMultiBody("initial_name");
  EXPECT_EQ(mb.getName(), "initial_name");

  mb.setName("new_name");
  EXPECT_EQ(mb.getName(), "new_name");

  mb.setName("robot_arm");
  EXPECT_EQ(mb.getName(), "robot_arm");
}

// Links cannot be reparented via Frame API
TEST(MultiBody, LinkReparentThrows)
{
  dart8::World world;
  auto robot = world.addMultiBody("robot");
  auto base = robot.addLink("base");
  auto child
      = robot.addLink("child", {.parentLink = base, .jointName = "joint"});

  EXPECT_THROW(child.setParentFrame(base), dart8::InvalidOperationException);
}

// Test MultiBody counts (currently empty)
TEST(MultiBody, EmptyCounts)
{
  dart8::World world;
  auto mb = world.addMultiBody("test");

  // Empty multibody has no links, joints, or DOFs
  EXPECT_EQ(mb.getLinkCount(), 0u);
  EXPECT_EQ(mb.getJointCount(), 0u);
  EXPECT_EQ(mb.getDOFCount(), 0u);
}

// Test multiple MultiBody instances in same World
TEST(MultiBody, MultipleInstances)
{
  dart8::World world;

  auto mb1 = world.addMultiBody("multibody1");
  auto mb2 = world.addMultiBody("multibody2");

  EXPECT_EQ(mb1.getName(), "multibody1");
  EXPECT_EQ(mb2.getName(), "multibody2");

  // Handles are independent
  mb1.setName("renamed1");
  EXPECT_EQ(mb1.getName(), "renamed1");
  EXPECT_EQ(mb2.getName(), "multibody2"); // Unchanged
}

//==============================================================================
// Joint and Link API Tests
//==============================================================================

// Test creating a single link (root link)
TEST(MultiBody, CreateRootLink)
{
  dart8::World world;
  auto robot = world.addMultiBody("robot");

  // Create root link (no parent joint)
  robot.addLink("base");

  EXPECT_EQ(robot.getLinkCount(), 1u);
  EXPECT_EQ(robot.getJointCount(), 0u);
}

// Test creating a simple 2-link chain with revolute joint
TEST(MultiBody, SimpleTwoLinkChain)
{
  dart8::World world;
  auto robot = world.addMultiBody("robot");

  // Create kinematic chain: base -> joint1 -> link1
  auto baseLink = robot.addLink("base");
  robot.addLink(
      "link1",
      {.parentLink = baseLink,
       .jointName = "joint1",
       .jointType = dart8::comps::JointType::Revolute});

  EXPECT_EQ(robot.getLinkCount(), 2u);
  EXPECT_EQ(robot.getJointCount(), 1u);
}

// Test creating a serial manipulator (typical robot arm)
TEST(MultiBody, SerialManipulator)
{
  dart8::World world;
  auto robot = world.addMultiBody("ur5");

  // Create 6-DOF serial manipulator: base -> 6 revolute joints -> 6 links
  auto base = robot.addLink("base");
  auto link1 = robot.addLink(
      "link1", {.parentLink = base, .jointName = "shoulder_pan"});
  auto link2 = robot.addLink(
      "link2", {.parentLink = link1, .jointName = "shoulder_lift"});
  auto link3
      = robot.addLink("link3", {.parentLink = link2, .jointName = "elbow"});
  auto link4
      = robot.addLink("link4", {.parentLink = link3, .jointName = "wrist1"});
  auto link5
      = robot.addLink("link5", {.parentLink = link4, .jointName = "wrist2"});
  robot.addLink("link6", {.parentLink = link5, .jointName = "wrist3"});

  EXPECT_EQ(robot.getLinkCount(), 7u);  // base + 6 links
  EXPECT_EQ(robot.getJointCount(), 6u); // 6 revolute joints
}

// Test creating prismatic joints
TEST(MultiBody, PrismaticJoints)
{
  dart8::World world;
  auto robot = world.addMultiBody("linear_stage");

  // Create 3-DOF Cartesian robot (XYZ linear stages)
  auto base = robot.addLink("base");
  auto linkX = robot.addLink(
      "link_x",
      {.parentLink = base,
       .jointName = "joint_x",
       .jointType = dart8::comps::JointType::Prismatic,
       .axis = Eigen::Vector3d::UnitX()});
  auto linkY = robot.addLink(
      "link_y",
      {.parentLink = linkX,
       .jointName = "joint_y",
       .jointType = dart8::comps::JointType::Prismatic,
       .axis = Eigen::Vector3d::UnitY()});
  robot.addLink(
      "link_z",
      {.parentLink = linkY,
       .jointName = "joint_z",
       .jointType = dart8::comps::JointType::Prismatic,
       .axis = Eigen::Vector3d::UnitZ()});

  EXPECT_EQ(robot.getLinkCount(), 4u);
  EXPECT_EQ(robot.getJointCount(), 3u);
}

// Test mixing revolute and prismatic joints
TEST(MultiBody, MixedJointTypes)
{
  dart8::World world;
  auto robot = world.addMultiBody("scara");

  // Create SCARA robot: 2 revolute + 1 prismatic + 1 revolute
  auto base = robot.addLink("base");
  auto link1 = robot.addLink(
      "link1",
      {.parentLink = base,
       .jointName = "joint1",
       .axis = Eigen::Vector3d::UnitZ()});
  auto link2 = robot.addLink(
      "link2",
      {.parentLink = link1,
       .jointName = "joint2",
       .axis = Eigen::Vector3d::UnitZ()});
  auto link3 = robot.addLink(
      "link3",
      {.parentLink = link2,
       .jointName = "joint3",
       .jointType = dart8::comps::JointType::Prismatic,
       .axis = Eigen::Vector3d::UnitZ()});
  robot.addLink(
      "link4",
      {.parentLink = link3,
       .jointName = "joint4",
       .axis = Eigen::Vector3d::UnitZ()});

  EXPECT_EQ(robot.getLinkCount(), 5u);
  EXPECT_EQ(robot.getJointCount(), 4u);
}

// Test custom joint axes
TEST(MultiBody, CustomJointAxes)
{
  dart8::World world;
  auto robot = world.addMultiBody("custom_axes");

  auto base = robot.addLink("base");

  // Joint rotating around custom axis
  Eigen::Vector3d customAxis(1.0, 1.0, 0.0);
  auto link1 = robot.addLink(
      "link1", {.parentLink = base, .jointName = "joint1", .axis = customAxis});

  // Joint translating along custom direction
  Eigen::Vector3d customDir(0.0, 1.0, 1.0);
  robot.addLink(
      "link2",
      {.parentLink = link1,
       .jointName = "joint2",
       .jointType = dart8::comps::JointType::Prismatic,
       .axis = customDir});

  EXPECT_EQ(robot.getLinkCount(), 3u);
  EXPECT_EQ(robot.getJointCount(), 2u);
}

// Test design-time restriction: cannot add joints after entering simulation
// mode
TEST(MultiBody, DesignTimeRestriction)
{
  dart8::World world;
  auto robot = world.addMultiBody("robot");

  // Create links in design mode
  auto base = robot.addLink("base");
  robot.addLink("link1", {.parentLink = base, .jointName = "joint1"});

  EXPECT_EQ(robot.getLinkCount(), 2u);
  EXPECT_EQ(robot.getJointCount(), 1u);

  // Enter simulation mode
  world.enterSimulationMode();
  EXPECT_TRUE(world.isSimulationMode());

  // Attempting to create link should throw
  EXPECT_THROW(robot.addLink("link2"), dart8::InvalidArgumentException);
}

// Test branching kinematic tree (not just serial chain)
TEST(MultiBody, BranchingKinematicTree)
{
  dart8::World world;
  auto robot = world.addMultiBody("humanoid");

  // Create simplified humanoid structure with branching
  auto torso = robot.addLink("torso");

  // Left arm branch
  auto leftShoulder = robot.addLink(
      "left_shoulder",
      {.parentLink = torso, .jointName = "left_shoulder_joint"});
  robot.addLink(
      "left_elbow",
      {.parentLink = leftShoulder, .jointName = "left_elbow_joint"});

  // Right arm branch
  auto rightShoulder = robot.addLink(
      "right_shoulder",
      {.parentLink = torso, .jointName = "right_shoulder_joint"});
  robot.addLink(
      "right_elbow",
      {.parentLink = rightShoulder, .jointName = "right_elbow_joint"});

  // Head branch
  auto neck
      = robot.addLink("neck", {.parentLink = torso, .jointName = "neck_joint"});
  robot.addLink("head", {.parentLink = neck, .jointName = "head_joint"});

  EXPECT_EQ(
      robot.getLinkCount(), 7u); // torso + 2 shoulders + 2 elbows + neck + head
  EXPECT_EQ(robot.getJointCount(), 6u); // 6 revolute joints
}

// Test that links persist across MultiBody handle copies
TEST(MultiBody, LinksPersistAcrossHandles)
{
  dart8::World world;
  auto robot1 = world.addMultiBody("robot");

  // Create links via robot1
  auto base = robot1.addLink("base");
  robot1.addLink("link1", {.parentLink = base, .jointName = "joint1"});

  // Get another handle to the same MultiBody
  auto robot2 = world.getMultiBody("robot");
  ASSERT_TRUE(robot2.has_value());

  // Both handles see the same links/joints
  EXPECT_EQ(robot1.getLinkCount(), 2u);
  EXPECT_EQ(robot2->getLinkCount(), 2u);
  EXPECT_EQ(robot1.getJointCount(), 1u);
  EXPECT_EQ(robot2->getJointCount(), 1u);
}
